#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S4,     angle,          sensorI2CCustomFastSkipStates)
#pragma config(Motor,  motorA,          C1,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          C2,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          C3,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     FL,            tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     FR,            tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     RL,            tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C1_2,     RR,            tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     Arm,           tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     Lift,          tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    SpinL,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    SpinR,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    GrabL,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    GrabR,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    Lift1,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth 96-2messages.

#define rightjoy joystick.joy1_y2/127.0*100.0
#define leftjoy joystick.joy1_y1/127.0*100.0

void conveyor(int power)
{
  motor[C1]=power;
  motor[C2]=power;
  motor[C3]=power;
}

const int lgrab=180;
const int lopen=100;
const int rgrab=115;
const int ropen=215;

ubyte I2Cmessage[4];


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  I2Cmessage[0]=3;
  I2Cmessage[1]=0x04;
  servo[SpinL]=128;
  servo[SpinR]=128;
  servo[GrabL]=lgrab;
  servo[GrabR]=rgrab;
  servo[Lift1]=230;
  servo[Lift2]=0;
  I2Cmessage[2]=0x42;
  I2Cmessage[3]=servo[SpinL];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x43;
  I2Cmessage[3]=servo[SpinR];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x44;
  I2Cmessage[3]=servo[GrabL];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x45;
  I2Cmessage[3]=servo[GrabR];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x46;
  I2Cmessage[3]=servo[Lift2];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x47;
  I2Cmessage[3]=servo[Lift1];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();
  I2Cmessage[2]=0x48;
  I2Cmessage[3]=0xAA;
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();
  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void servoupdate()
{
  I2Cmessage[2]=0x42;
  I2Cmessage[3]=servoes[0];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x43;
  I2Cmessage[3]=servoes[1];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x44;
  I2Cmessage[3]=servoes[2];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x45;
  I2Cmessage[3]=servoes[3];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x46;
  I2Cmessage[3]=servoes[5];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();

  I2Cmessage[2]=0x47;
  I2Cmessage[3]=servoes[4];
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();
  I2Cmessage[2]=0x48;
  I2Cmessage[3]=0xAA;
  hogCPU();
  sendI2CMsg(S1,I2Cmessage[0],0);
  releaseCPU();
}

typedef struct
{
  bool pressed;
  bool open;
} servobuttons;

servobuttons front;
servobuttons top;
servobuttons middle;



task main()
{
  initializeRobot();
  waitForStart();   // wait for start of tele-op phase
  bool flipped=false;
    bool notinitial=false;
  bool slow=false;
  while (true)
  {
    ///////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////
    ////                                                   ////
    ////      Add your robot specific tele-op code here.   ////
    ////                                                   ////
    ///////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.

    getJoystickSettings(joystick);
  servoupdate();
    if(joy1Btn(7)&&!flipped)
    {
      flipped=true;
      slow=!slow;
    }
    if(!joy1Btn(7))
    {
      flipped=false;
    }
    if(!slow)
    {
      if (abs(leftjoy)>10)
      {
        motor[FL]=leftjoy;
        motor[RL]=leftjoy;
      }
      else
      {
        motor[FL]=0;
        motor[RL]=0;
      }
      if (abs(rightjoy)>10)
      {
        motor[FR]=rightjoy;
        motor[RR]=rightjoy;
      }
      else
      {
        motor[FR]=0;
        motor[RR]=0;
      }
    }
    //slow
    else
    {
      if (abs(leftjoy)>10)
      {
        motor[FL]=leftjoy/3;
        motor[RL]=leftjoy/3;
      }
      else
      {
        motor[FL]=0;
        motor[RL]=0;
      }
      if (abs(rightjoy)>10)
      {
        motor[FR]=rightjoy/3;
        motor[RR]=rightjoy/3;
      }
      else
      {
        motor[FR]=0;
        motor[RR]=0;
      }
    }
    if (joy1Btn(6))
    {
      motor[Arm]=100;
    }
    else if (joy1Btn(5))
    {
      motor[Arm]=-100;
    }
    else if (joy2Btn(6))
    {
      motor[Arm]=100;
    }
    else if (joy2Btn(8))
    {
      motor[Arm]=-100;
    }
    else
    {
      motor[Arm]=0;
    }
    if (joy2Btn(7))
    {
      conveyor(100);
    }
    else if (joy2Btn(5))
    {
      conveyor(-100);
    }
    else
    {
      conveyor(0);
    }
    bool frontpressed2;
    if(joy1Btn(1)&&!frontpressed2)
    {
      frontpressed2=true;
      front.open=!front.open;
    }
    if(!joy1Btn(1))
    {
      frontpressed2=false;
    }
    if(joy2Btn(1)&&!front.pressed)
    {
      front.pressed=true;
      front.open=!front.open;
    }
    if(!joy2Btn(1))
    {
      front.pressed=false;
    }
    if (front.open)
    {
      servoes[2]=lopen;
      servoes[3]=ropen;
    }
    if (!front.open)
    {
      servoes[2]=lgrab;
      servoes[3]=rgrab;
    }

    servoes[0]=joystick.joy2_y1/6.4+128;
    if (joy1Btn(3))
    {
      motor[Lift]=100;
    }
    else if (joy1Btn(2))
    {
      motor[Lift]=-100;
    }
    else if (joy2Btn(3))
    {
      motor[Lift]=100;
    }
    else if (joy2Btn(2))
    {
      motor[Lift]=-100;
    }
    else
    {
      motor[Lift]=0;
    }

    if((joystick.joy2_TopHat==0)&&!top.pressed)
    {
      notinitial=true;
      top.pressed=true;
      top.open=!top.open;
    }
    if(!(joystick.joy2_TopHat==0))
    {
      top.pressed=false;
    }
    if (top.open)
    {
      servoes[4]=65;
      servoes[5]=205;
    }

    if (!top.open)
    {
      servoes[4]=150;
      servoes[5]=140;
    }
        if(!notinitial)
    {
        servoes[4]=250;
  servoes[5]=60;
    }
    if((joy1Btn(4))&&!middle.pressed)
    {
      middle.pressed=true;
      middle.open=!middle.open;
    }
    if(!(joy1Btn(4)))
    {
      middle.pressed=false;
    }
    if (middle.open)
    {
      servoes[1]=245;
    }
    if (!middle.open)
    {
      servoes[1]=110;
    }
  }
}
